#include "task.h"
#include <string.h>
#include "common.h"
#include "dynamixel/dynamixel.h"

#define TASK_BAUDRATE		576000
#define TASK_TX_PIN			GPIO_Pin_10
#define TASK_TX_PinSource    GPIO_PinSource10

#define TASK_RX_PIN			GPIO_Pin_11
#define TASK_RX_PinSource    GPIO_PinSource11

#define	TASK_PORT			GPIOB
#define	TASK_IO_CLK_EN		RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE)

#define TASK_UART			USART3
#define TASK_UART_CLK_EN		RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE)

#define TASK_GPIO_AF_UART 		GPIO_AF_USART3

#define PKT_TYPE	2
#define PKT_LEN_L	3
#define PKT_LEN_H	4
#define PKT_PARAM_START	5

#define LEAST_PKT_LEN 7


uint8_t time_to_request = RESET;

uint8_t	task_timeout_cnt=100;
uint8_t task_current_cnt=0;

uint8_t task_write_buff[MAX_BUFF_LEN] = {0x5A, 0xA5};
uint8_t task_read_buff[MAX_BUFF_LEN] = {0};


uint32_t make_send_buff(uint8_t type, uint8_t *data, uint16_t len)
{
	uint16_t i;
	uint16_t crc=0;
	task_write_buff[PKT_TYPE] = type;
	task_write_buff[PKT_LEN_L] = DXL_LOBYTE(len);
	task_write_buff[PKT_LEN_H] = DXL_HIBYTE(len);
	for(i=0; i<len; i++)
	{
		task_write_buff[i+PKT_PARAM_START] = data[i];
	}
	crc16_update(&crc, task_write_buff+PKT_TYPE, len+3);
	task_write_buff[len+PKT_PARAM_START] = DXL_LOBYTE(crc);
	task_write_buff[len+PKT_PARAM_START+1] = DXL_HIBYTE(crc);
	return LEAST_PKT_LEN+len;
}

void task_request(uint8_t pwr, uint8_t btn1, uint8_t btn2, uint8_t *e)
{
	uint8_t data[9] = {pwr, btn1, btn2};
	memcpy(data+3, e, 6);
	uint32_t len;
	len = make_send_buff(REQ_DATA, data, 9);
	Task_Port_Write(task_write_buff, len);
}

void send_task_ack()
{
	uint32_t len = make_send_buff(ACK_DATA, 0, 0);
	Task_Port_Write(task_write_buff, len);
}

enum
{
    Status_5A,
    Status_A5,
	Status_Type,
	Status_LenL,
	Status_LenH,
    Status_Data
};

void TaskPkt_Decode(uint8_t c)
{
	static uint16_t recv_len=0;
	static uint16_t crc=0;
	static uint8_t recv_status = Status_5A;
	static uint16_t pkt_len = 0;
	uint8_t i;

	switch(recv_status)
	{
	case Status_5A:
		if(c==0x5A)
		{
			recv_len = 0;
			recv_status = Status_A5;
			task_read_buff[recv_len++] = c;
		}
		break;
	case Status_A5:
		if(c==0xA5)
		{
			recv_status = Status_Type;
			task_read_buff[recv_len++] = c;
		}
		else
		{
			recv_status = Status_5A;
			recv_len = 0;
		}
		break;
	case Status_Type:
		task_read_buff[recv_len++] = c;
		recv_status = Status_LenL;
		break;
	case Status_LenL:
		task_read_buff[recv_len++] = c;
		recv_status = Status_LenH;
		break;
	case Status_LenH:
		pkt_len = DXL_MAKEWORD(task_read_buff[recv_len-1], c);
		task_read_buff[recv_len++] = c;
		recv_status = Status_Data;
		break;
	case Status_Data:
		task_read_buff[recv_len++] = c;
		break;
	default:
		recv_status = Status_5A;
		break;
	}
	//GPIO_ToggleBits(GPIOE, GPIO_Pin_4);
	if(recv_len >= LEAST_PKT_LEN+pkt_len)
	{
		crc = 0;
		crc16_update(&crc, task_read_buff+PKT_TYPE, pkt_len+3);
		if(crc == DXL_MAKEWORD(task_read_buff[recv_len-2], task_read_buff[recv_len-1]))
		{
			if(task_read_buff[PKT_TYPE] == MOTOR_DATA)
			{
				i = 0;
				while(i<TOTAL_MOTOR_NUM)
				{
					uint8_t *ptr=task_read_buff+PKT_PARAM_START+i*5;
					dxl_ids[i] = *ptr;
					ptr++;
					dxl_positions[i] = DXL_MAKEDWORD(DXL_MAKEWORD(*ptr, *(ptr+1)), DXL_MAKEWORD(*(ptr+2), *(ptr+3)));
					i++;
				}
			}
			else if(task_read_buff[PKT_TYPE] == LED_DATA)
			{
				GPIO_WriteBit(GPIOE, GPIO_Pin_2, task_read_buff[PKT_PARAM_START]);
				GPIO_WriteBit(GPIOE, GPIO_Pin_4, task_read_buff[PKT_PARAM_START+1]);
			}
		}
		memset(task_read_buff, 0, MAX_BUFF_LEN);
		recv_len = 0;
		recv_status = Status_5A;
		pkt_len = 0;
	}
}

void Task_Init()
{
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	TASK_UART_CLK_EN;
	TASK_IO_CLK_EN;

	GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_UP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

	GPIO_InitStructure.GPIO_Pin = TASK_TX_PIN;
	GPIO_Init(TASK_PORT, &GPIO_InitStructure);
	GPIO_InitStructure.GPIO_Pin = TASK_RX_PIN;
	GPIO_Init(TASK_PORT, &GPIO_InitStructure);

	GPIO_PinAFConfig(TASK_PORT,TASK_TX_PinSource,TASK_GPIO_AF_UART);
	GPIO_PinAFConfig(TASK_PORT,TASK_RX_PinSource,TASK_GPIO_AF_UART);

	USART_InitStructure.USART_BaudRate 	 = TASK_BAUDRATE;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits   = USART_StopBits_1;
	USART_InitStructure.USART_Parity     = USART_Parity_No ;
	USART_InitStructure.USART_Mode 	    = USART_Mode_Rx | USART_Mode_Tx;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;

	USART_Init(TASK_UART,&USART_InitStructure);

	NVIC_InitStructure.NVIC_IRQChannel = USART3_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);

	USART_ITConfig(TASK_UART, USART_IT_RXNE, ENABLE);
	USART_Cmd(TASK_UART,ENABLE);
}

int Task_Port_Write(uint8_t *data, uint32_t length)
{
	uint32_t i;
	for(i=0; i<length; i++)
	{
		USART_SendData(TASK_UART, *(data+i));
		while(USART_GetFlagStatus(TASK_UART, USART_FLAG_TC) == RESET){}
	}
	return i;
}

int Task_Port_Read(uint8_t *data, uint32_t length)
{
	uint32_t i=0, recv_len=0;
	for(i=0; i<100*length && recv_len<length; i++)
	{
		if(USART_GetFlagStatus(TASK_UART, USART_FLAG_RXNE) == SET)
		{
			*(data+recv_len) = USART_ReceiveData(TASK_UART);
			recv_len++;
		}
		task_current_cnt++;
	}
	return recv_len;
}
